#ifndef _DEFINE_H_
#define _DEFINE_H_

// Port
#define MOTOR_PORT_LEFT 		3
#define MOTOR_PORT_TOWER 		4
#define MOTOR_PORT_LED	 		2
#define MOTOR_PORT_RIGHT 		5
#define LED_UNDER_FRONTLEFT		9
#define LED_UNDER_FRONTRIGHT	12
#define LED_UNDER_BACKRIGHT		15
#define LED_UNDER_CENTER		8
#define LED_LEFT_FRONT			10
#define LED_LEFT_BACK			14
#define LED_RIGHT_FRONT			11
#define LED_RIGHT_BACK			13
#define GYRO_PORT 				23
#define ENCODER_PORT_LEFT 		27
#define ENCODER_PORT_RIGHT		25
#define SERVO_SCORE				4
#define SERVO_SKUNK				5
#define BUMPER_WALL_LEFT		6
#define BUMPER_WALL_RIGHT		0
#define BUMPER_BACK_RIGHT		2	
#define BUMPER_BACK_LEFT		4	
#define BUMPER_FRONT			1

#define DISTANCE_PORT			17

// Motor
#define STRAIGHT_MOTOR_VEL		150
#define WALL_MOTOR_VEL			150
#define LINE_MOTOR_VEL			120
#define ENEMY_MOTOR_VEL			255
#define MAX_TURN_MOTOR_VEL		255
#define MIN_TURN_MOTOR_VEL		255
#define MOTOR_MAX_VEL			255


// Sharp Distance
#define DISTANCE_THRESHOLD		6.5
#define SHARP_M		22840
#define SHARP_C		26

// Gyro
#define LSB_MS_PER_DEG	1062.0
#define GYRO_CALI_TIME	3500

// Encoder
#define SKUNK_LEFT_ENC			100 // change this!!!

// Turn Constants
#define SHAFT_PER_WHEEL		90
#define WHEEL_RADIUS		1.75
#define DIST_PER_WHEEL		11.0		// WHEEL_RADIUS * 2 * M_PI
#define ROBOT_WIDTH			8.25

// ORIENTATION CONSTANT -- according to README illustration
#define NORTH	0
#define EAST	1
#define SOUTH	2
#define WEST	3

//Time Limit
#define TURN_TIME_LIMIT			1500 // ms
#define ENEMY_TIME_LIMIT		1500 // ms
#define OPP_TIME_LIMIT			1500 // ms
#define SCORE_TIME_LIMIT		5000 // ms
#define SKUNK_TIME_LIMIT		5000 // ms
#define WALK_TIME_LIMIT			5000 // ms

// Variables
uint8_t 	goalPoint;
uint8_t 	pointToDo[3];		// [0] = 4 [1] = 2 [2] = 1
uint8_t 	initPlace;			// 0 White ,1 Black
uint8_t		initOrient;			// 0,1,2,3
uint16_t	threshold_led[8];	// {ufr, ufl, ubr, ufc, lf, lb, rf, rb} -- ufr = Under - Front - Right
uint16_t 	shaftValueMonitorL; 
uint16_t 	shaftValueMonitorR;
// Additional variables
uint8_t		currentSide;		// 0,1,2,3,4 (0 = N, 1 = E, 2 = S, 3 = W, 4 = middle)
uint8_t		currentOrient;		// 0,1,2,3

uint8_t	unfullfill_state[10];
uint8_t	state;
	/*	#  is going to do something
		0  get away from opponent
		1  goto opponent goal 4
		2  walk pass the goal
		3  score goal 2
		6  turn at the opponent goal 1
		9  catch skunk
		12 turn after catching skunk
		15 score goal 1
		21 score goal 4
		24 go to opponent home
		27 done
	*/
uint8_t		myStateStatus;	// 0 = in progress, 1 = finish the state
uint8_t		oldState;	// use when get away from enemy
uint8_t		enemyCount;	// use when get away from enemy
uint8_t		status; // to know what it is doing right now
int8_t 		motorVelNOWL;
int8_t 		motorVelNOWR;
/* 
 * 0 = Do nothing
 * 1 = walking
 * 2 = turning
*/
// for catch skunk
uint16_t	skunk_stop_enc;

// stuck
uint16_t	check_stuck;	// 0 = clear, 1 = stuck

// backToWall
uint16_t	backToWallTimeout; 

#endif
